/****************************************************************************

				?Cambridge Silicon Radio Ltd, 2006
 	 				
				All rights reserved

FILE:			hci_bccmd.cxx

DESCRIPTION:	BCCMD implementation

REVISION:		$Revision: #1 $

****************************************************************************/
#include <windows.h>
#include <stdio.h>
#include <tchar.h>
#include "hci_bccmd.h"

#define	CSR_GENERAL					TEXT("Software\\CSR\\General")
#define	CSR_HW_BLUETOOTH_ADDRESS	TEXT("HardwareBluetoothAddress")

#define ADD_UINT16(b,v) *b++ = (v & 0xFF); *b++ = (v >> 8 & 0xFF);
#define GET_UINT16(b) (*b++)

#define PSKEY_FILE_LINE_SIZE	2000

typedef struct 
{
	uint16_t	type;
	uint16_t	length;
	uint16_t	seqNumber;  
	uint16_t	varid;
	uint16_t	errorCode;
} BccmdHeaderT;

static int Hex2Int(char c)
{
	if (c >= '0' && c <= '9')
		return c - '0';
	if (c >= 'A' && c <= 'F')
		return c - 'A' + 10;
	if (c >= 'a' && c <= 'f')
		return c - 'a' + 10;
	return -1;
}

Bccmd::Bccmd():
	seqNumber(0)
{

}

Bccmd::~Bccmd()
{

}

DWORD Bccmd::ConfigureChip(TCHAR* filename)
{
	FILE*	stream;
	DWORD	result = ERROR_FILE_NOT_FOUND;

	if (stream = _tfopen(filename, TEXT("r")))
	{
		result = ParsePSRFile(stream);

		fclose(stream);
	}

	return result;
}

DWORD Bccmd::DumpKeys(TCHAR* filename)
{
	FILE*		stream;
	DWORD		result = ERROR_FILE_NOT_FOUND;
	uint16_t	key = 0;
	uint32_t	size, index;
	uint16_t	buffer[256];
	
	if (stream = _tfopen(filename, TEXT("w")))
	{
		while (GetNextPskey(key, &key) == ERROR_SUCCESS)
		{
			if (GetPskey(key, buffer, &size) == ERROR_SUCCESS)
			{
				fprintf(stream, "Key %04x: ", key);

				for (index = 0; index < (size >> 1); index++)
				{
					fprintf(stream, "%04x ", buffer[index]);
				}

				fprintf(stream, "\n");
			}
		}


		fclose(stream);
	}

	return result;
}

DWORD Bccmd::ParsePSRFile(FILE* ist)
{
	char		linebuf[PSKEY_FILE_LINE_SIZE];
	uint16_t	data[PSKEY_FILE_LINE_SIZE];
	uint16_t*	pdata;
	char*		p;
	char*		end;
	int			key;
	HKEY		hk;
	bool		query_mode = false;
	DWORD		result = ERROR_SUCCESS;
	unsigned __int64	bluetoothAddress;

	while (fgets(linebuf, PSKEY_FILE_LINE_SIZE, ist))
	{
		// Does it start with an &?
		p = linebuf;
		linebuf[PSKEY_FILE_LINE_SIZE - 1] = '\0';
		end = &linebuf[strlen(linebuf)];
		while ((p < end) && isspace(*p))
			p++;
		if (p == end)
			continue;
		if (*p == '?')
			query_mode = true;
		else if (*p != '&')
			continue; // Not a valid line.
		p++;
		
		// Are there digits next?
		key = 0;
		while ((p < end) && isxdigit(*p))
		{
			key = (key * 16) + Hex2Int(*p);
			p++;
		}
		if (key == 0)
			continue;
		while ((p < end) && isspace(*p))
			p++;
		if (p == end)		
		{   // No operation?  
			continue;  // Next line please.
		}

		// Read the operation
		if (*p != '=')
			continue;  // Next line please.

		if (p < end)
			p++;
    
		// Read the data
		pdata = data;
		for (;;)
		{
			while ((p < end) && isspace(*p))
				p++;
			if (p == end)
				break;
			if (*p == '\"') {
				p++;
				while (p < end) {	
					*pdata++ = *p++;
				}
				break;
			} else {
				if (p >= end || (!isxdigit(*p)))
					break;
				*pdata = 0;
				while ((p < end) && isxdigit(*p)) {
					*pdata = (*pdata * 16) + Hex2Int(*p);
					p++;
				}	
				pdata++;
			}
		}

		if (key == 0x0001)
		{
			char buffer[8];
			BOOL regKeyFound = FALSE;

			if (ERROR_SUCCESS == RegOpenKeyEx( HKEY_LOCAL_MACHINE, CSR_GENERAL, 0, KEY_READ, &hk))
			{
				DWORD				dwType = 0;
				WCHAR				szString[13];
				DWORD				dwSize = sizeof(szString);
			
				if (ERROR_SUCCESS == RegQueryValueEx( hk, CSR_HW_BLUETOOTH_ADDRESS, NULL, &dwType, (BYTE *)szString, &dwSize)) 
				{
					_tcslwr(szString);
					if (_stscanf((TCHAR*) szString, TEXT("%I64x"), &bluetoothAddress) && bluetoothAddress)
					{
						buffer[0] = (char)(bluetoothAddress & 0x0000000000FF0000L) >> 16;
						buffer[1] = 0;
						buffer[2] = (char)(bluetoothAddress & 0x00000000000000FFL);
						buffer[3] = (char)(bluetoothAddress & 0x000000000000FF00L) >> 8;
						buffer[4] = (char)(bluetoothAddress & 0x00000000FF000000L) >> 24;
						buffer[5] = 0;
						buffer[6] = (char)(bluetoothAddress & 0x000000FF00000000L) >> 32;
						buffer[7] = (char)(bluetoothAddress & 0x0000FF0000000000L) >> 40;
						regKeyFound = TRUE;
					}
				}
				RegCloseKey(hk);

			}

			if (regKeyFound)
			{
				if ((result = SetPskey(key, (uint16_t*) buffer, 8)) != ERROR_SUCCESS)
				{
					break;
				}
			}
			else
			{
				if ((result = SetPskey(key, data, (pdata - data) << 1)) != ERROR_SUCCESS)
				{
					break;
				}
			}
		}
		else if ((result = SetPskey(key, data, (pdata - data) << 1)) != ERROR_SUCCESS)
		{
			break;
		}
	}

	return result;
}

uint16_t* Bccmd::ParseHeader(uint16_t* buffer, uint32_t size, BccmdStatusT* status)
{
	BccmdHeaderT*	bccmdHeader = (BccmdHeaderT*) buffer;
	uint16_t*		result = NULL;
	
	if (!buffer || size < 10 || bccmdHeader->type != BCCMD_GETRESP || bccmdHeader->seqNumber != seqNumber)
	{
		*status = BCCMD_STATUS_BAD_RESPONSE;	
	}
	else
	{
		*status = (BccmdStatusT) bccmdHeader->errorCode;
		result = (uint16_t*) (bccmdHeader + 1);
	}
	
	seqNumber++;

	return result;
}

uint8_t* Bccmd::PrepareHeader(uint8_t* buffer, BccmdTypeT type, uint16_t length, uint16_t varid, BccmdStatusT status)
{
	ADD_UINT16(buffer, type);
	ADD_UINT16(buffer, length);
	ADD_UINT16(buffer, seqNumber);
	ADD_UINT16(buffer, varid);
	ADD_UINT16(buffer, BCCMD_STATUS_OK);

	return buffer;
}

DWORD Bccmd::Bootstrap()
{
	return ERROR_SUCCESS;
}

DWORD Bccmd::GetNextPskey(uint16_t key, uint16_t* nextKey)
{
	unsigned char	bccmdBuffer[256];
	uint8_t*		payload;
	uint16_t*		outPointer;
	unsigned char*	outBuffer = NULL;
	uint32_t		outSize;
	uint16_t		keyLength = 0;
	BccmdStatusT	status;

	payload = PrepareHeader((uint8_t*) bccmdBuffer, (BccmdTypeT) BCCMD_GETREQ, 9, 0x3005, BCCMD_STATUS_OK);
	ADD_UINT16(payload, key);
	ADD_UINT16(payload, 0x0000);
	ADD_UINT16(payload, 0x0000);
	ADD_UINT16(payload, 0x0000);

	if (BCCMDTransaction((uint8_t*) bccmdBuffer, payload - bccmdBuffer, &outBuffer, &outSize) != ERROR_SUCCESS)
	{
		return ERROR_SERVICE_NOT_ACTIVE;
	}
	
	outPointer = ParseHeader((uint16_t*) outBuffer, outSize, &status);
	if (!outPointer || status != BCCMD_STATUS_OK)
	{
		free(outBuffer);
		return ERROR_INVALID_DATA;	
	}

	GET_UINT16(outPointer);
	GET_UINT16(outPointer);

	*nextKey = GET_UINT16(outPointer);
	if (!*nextKey)
	{
		return ERROR_NO_DATA;
	}

	free(outBuffer);

	return ERROR_SUCCESS;
}

DWORD Bccmd::GetPskey(uint16_t key, uint16_t* buffer, uint32_t* size, uint16_t store)
{
	unsigned char	bccmdBuffer[512];
	unsigned char*	payload;
	uint16_t*		outPointer;
	unsigned char*	outBuffer = NULL;
	uint32_t		outSize = 0;
	uint32_t		index;
	uint16_t		keyLength = 0;
	BccmdStatusT	status;

	payload = PrepareHeader((uint8_t*) bccmdBuffer, (BccmdTypeT) BCCMD_GETREQ, 9, 0x3006, BCCMD_STATUS_OK);
	ADD_UINT16(payload, key);
	ADD_UINT16(payload, 0x0000);
	ADD_UINT16(payload, 0x0000);
	ADD_UINT16(payload, 0x0000);
	
	if (BCCMDTransaction((uint8_t*) bccmdBuffer, payload - bccmdBuffer, &outBuffer, &outSize) != ERROR_SUCCESS)
	{
		return ERROR_SERVICE_NOT_ACTIVE;
	}
	
	if (outBuffer && outSize)
	{
		outPointer = ParseHeader((uint16_t*) outBuffer, outSize, &status);
		if (!outPointer || status != BCCMD_STATUS_OK)
		{
			free(outBuffer);
			return ERROR_INVALID_DATA;	
		}
		
		if (GET_UINT16(outPointer) != key)
		{
			free(outBuffer);
			return ERROR_OUT_OF_STRUCTURES;	
		}

		keyLength = GET_UINT16(outPointer);
	}

	free(outBuffer);
	outBuffer = NULL;
		
	if (keyLength && *size >= (uint32_t) (keyLength << 1))
	{
		payload = PrepareHeader((uint8_t*) bccmdBuffer, (BccmdTypeT) BCCMD_GETREQ, 8 + keyLength, 0x7003, BCCMD_STATUS_OK);
		ADD_UINT16(payload, key);
		ADD_UINT16(payload, keyLength);
		ADD_UINT16(payload, store);
	
		for (index = 0; index < keyLength; index++)
		{
			ADD_UINT16(payload, 0x0000);
		}

		if (BCCMDTransaction((uint8_t*) bccmdBuffer, payload - bccmdBuffer, &outBuffer, &outSize) != ERROR_SUCCESS)
		{
			return ERROR_SERVICE_NOT_ACTIVE;
		}
		
		outPointer = ParseHeader((uint16_t*) outBuffer, outSize, &status);
		if (!outPointer || status != BCCMD_STATUS_OK)
		{
			free(outBuffer);
			return ERROR_INVALID_DATA;	
		}
		
		if (*size < (uint32_t) (keyLength << 1))
		{
			return ERROR_OUT_OF_STRUCTURES;
		}

		GET_UINT16(outPointer);
		GET_UINT16(outPointer);
		GET_UINT16(outPointer);

		for (index = 0; index < keyLength; index++)
		{
			*buffer++ = GET_UINT16(outPointer);
		}

		*size = (keyLength << 1);

		free(outBuffer);
	}
	else
	{
		return ERROR_NOT_ENOUGH_MEMORY;
	}

	return ERROR_SUCCESS;	
}

DWORD Bccmd::SetPskey(uint16_t key, uint16_t* buffer, uint32_t size)
{
	unsigned char	bccmdBuffer[256];
	unsigned char*	payload;
	uint16_t*		outPointer;
	unsigned char*	outBuffer = NULL;
	uint32_t		outSize = 0;
	uint32_t		index;
	uint16_t		keyLength = 0;
	uint16_t		value;
	BccmdStatusT	status;

	keyLength = size >> 1;

	if (keyLength)
	{
		payload = PrepareHeader((uint8_t*) bccmdBuffer, (BccmdTypeT) BCCMD_SETREQ, 8 + keyLength, 0x7003, BCCMD_STATUS_OK);
		ADD_UINT16(payload, key);
		ADD_UINT16(payload, keyLength);
		ADD_UINT16(payload, 0x0008);
	
		for (index = 0; index < keyLength; index++)
		{
			value = *buffer++;
			ADD_UINT16(payload, value);
		}

		if (BCCMDTransaction((uint8_t*) bccmdBuffer, payload - bccmdBuffer, &outBuffer, &outSize) != ERROR_SUCCESS)
		{
			return ERROR_SERVICE_NOT_ACTIVE;
		}
		
		outPointer = ParseHeader((uint16_t*) outBuffer, outSize, &status);
		if (!outPointer || status != BCCMD_STATUS_OK)
		{
			free(outBuffer);
			return ERROR_INVALID_DATA;	
		}

		free(outBuffer);
	}

	return ERROR_SUCCESS;
}

DWORD Bccmd::StartRadioTest()
{
	unsigned char	bccmdBuffer[256] = {0};
	unsigned char*	payload;
	unsigned char*	outBuffer = NULL;
	uint32_t		outSize = 0;
	uint16_t*		outPointer;
	BccmdStatusT	status;

	payload = PrepareHeader((uint8_t*) bccmdBuffer, (BccmdTypeT) BCCMD_SETREQ, 9, 0x5004, BCCMD_STATUS_OK);
	ADD_UINT16(payload, 0x0001);
	ADD_UINT16(payload, 0x0989);
	ADD_UINT16(payload, 0xff32);
	ADD_UINT16(payload, 0x0000);

	if (BCCMDTransaction((uint8_t*) bccmdBuffer, payload - bccmdBuffer, &outBuffer, &outSize) != ERROR_SUCCESS)
	{
		return ERROR_SERVICE_NOT_ACTIVE;
	}

	outPointer = ParseHeader((uint16_t*) outBuffer, outSize, &status);
	if (!outPointer || status != BCCMD_STATUS_OK)
	{
		free(outBuffer);
		return ERROR_INVALID_DATA;	
	}

	free(outBuffer);

	return ERROR_SUCCESS;
}

DWORD Bccmd::ColdReset()
{
	unsigned char	bccmdBuffer[256] = {0};
	unsigned char*	payload;
	unsigned char*	outBuffer = NULL;
	uint32_t		outSize = 0;

	payload = PrepareHeader((uint8_t*) bccmdBuffer, (BccmdTypeT) BCCMD_SETREQ, 9, 0x4001, BCCMD_STATUS_OK);
	ADD_UINT16(payload, 0);
	ADD_UINT16(payload, 0);
	ADD_UINT16(payload, 0);
	ADD_UINT16(payload, 0);

	if (BCCMDTransaction((uint8_t*) bccmdBuffer, payload - bccmdBuffer, &outBuffer, &outSize) != ERROR_SUCCESS)
	{
		return ERROR_SERVICE_NOT_ACTIVE;
	}

	return ERROR_SUCCESS;
}

DWORD Bccmd::WarmReset()
{
	unsigned char	bccmdBuffer[256] = {0};
	unsigned char*	payload;
	unsigned char*	outBuffer = NULL;
	uint32_t		outSize = 0;

	payload = PrepareHeader((uint8_t*) bccmdBuffer, (BccmdTypeT) BCCMD_SETREQ, 9, 0x4002, BCCMD_STATUS_OK);
	ADD_UINT16(payload, 0);
	ADD_UINT16(payload, 0);
	ADD_UINT16(payload, 0);
	ADD_UINT16(payload, 0);

	if (BCCMDTransaction((uint8_t*) bccmdBuffer, payload - bccmdBuffer, &outBuffer, &outSize) != ERROR_SUCCESS)
	{
		return ERROR_SERVICE_NOT_ACTIVE;
	}

	return ERROR_SUCCESS;
}
